#define NCTRLPOINTS 5 
 
                  
#extension GL_ARB_texture_rectangle : enable 
uniform sampler2DRect P;
uniform sampler2DRect Q;
uniform sampler2D     positions;
uniform sampler2D     normals;

/// constant values for cubic equation computing
const float K1DIV3   = 0.3333333;    /// 1/3
const float K1DIV9   = 0.1111111;    /// 1/9
const float K1DIV54  = 0.0185185;    /// 1/54
const float EPSILON  = 1.0E-5;      

float cubicroot(in float a, in float b, in float c) {
   
   const float Q = (3.0*b - a*a)*K1DIV9;
   const float R = (9.0*a*b - 27.0*c - 2.0*a*a*a)*K1DIV54;
   const float D = Q*Q*Q + R*R;
   const float sqrtD = sqrt(D);
   
   if (D>=0.0) {  /// one real, two complex :: D==0 root reals, at least two are equal 
      const float S = sign(R+sqrtD)*pow(abs(R+sqrtD), K1DIV3);
      const float T = sign(R-sqrtD)*pow(abs(R-sqrtD), K1DIV3);
      return -a*K1DIV3 + S + T;
   }
   else {   /// all root are real and unequal
      const float theta = acos(R/sqrt(-Q*Q*Q));
      return 2.0*sqrt(-Q)*cos(theta*K1DIV3)-a*K1DIV3;
   }
}

float weight(in vec3 p, in int i) {
   
   vec3 vd = texture2DRect(P, vec2(0.0, i)).xyz - p;
   float denom = dot(vd,vd);
   return 1.0/(denom + 0.01*0.01);
}

mat3 correlationMatrix(const in vec3 p, const in vec3 pstar, const in vec3 qstar) {
   
   vec3 pjhatT, qjhat; mat3 M = mat3(0.0);
   for (int j=0; j<NCTRLPOINTS; j++) {
      pjhatT  = (texture2DRect(P, vec2(0.0, j))).xyz - pstar;
      qjhat   = (texture2DRect(Q, vec2(0.0, j))).xyz - qstar;
      pjhatT = pjhatT*weight(p,j);
      M[0] += (pjhatT*qjhat.x);
      M[1] += (pjhatT*qjhat.y);
      M[2] += (pjhatT*qjhat.z);
   }
   return M;
}

void weightedCentroids(const in vec3 p, out vec3 pstar, out vec3 qstar) {
   
   float Wt=0.0, W; pstar=vec3(0.0), qstar=vec3(0.0);
   for (int i=0; i<NCTRLPOINTS; i++) {
      W = weight(p,i);
      Wt += W;
      pstar += W*(texture2DRect(P, vec2(0.0, i))).xyz;
      qstar += W*(texture2DRect(Q, vec2(0.0, i))).xyz;
   }
   pstar /= Wt;
   qstar /= Wt;
}

float largestRoot(const mat3 M) {
   
   const float detM = M[0][0]*(M[1][1]*M[2][2]-M[1][2]*M[2][1])+
                      M[0][1]*(M[1][2]*M[2][0]-M[1][0]*M[2][2])+
                      M[0][2]*(M[1][0]*M[2][1]-M[2][0]*M[1][1]);
   
   /// setting the cubic equation
   float normaM[3], normaM2=0.0, prodNorma2=0.0;
   normaM[0] = dot(M[0],M[0]);
   normaM2 += normaM[0];
   normaM[1] = dot(M[1],M[1]);
   normaM2 += normaM[1];
   normaM[2] = dot(M[2],M[2]);
   normaM2 += normaM[2];
   
   prodNorma2 += normaM[0]*normaM[1];
   prodNorma2 += normaM[1]*normaM[2];
   prodNorma2 += normaM[2]*normaM[0];
   
   float prodesc, prodesc2=0.0;
   prodesc = dot(M[0],M[1]);
   prodesc2 += prodesc*prodesc;
   prodesc = dot(M[1],M[2]);
   prodesc2 += prodesc*prodesc;
   prodesc = dot(M[2],M[0]);
   prodesc2 += prodesc*prodesc;
   
   const float a = -4.0*normaM2;
   const float b = 16.0*(prodNorma2-prodesc2);
   const float c = -64.0*detM*detM;
   /// getting the cubic root
   float p2 = cubicroot(a,b,c); 
   const float _p = sqrt(p2);
   const float minusp2_plus_4M2 = -p2 + 4.0*normaM2;
   const float _16detMdivp = (16.0*detM)/_p;
   
   /// the largest quartic root
   float maxroot = max(-_p + sqrt(minusp2_plus_4M2 - _16detMdivp), 
                        _p + sqrt(minusp2_plus_4M2 + _16detMdivp));
   
   return maxroot*0.5;
}

vec3 rotationVector(const in mat3 M, const in float eigenvalue) {
   
   /// solving the 3x3 linear system: (N-eigenvalue*I) e = V
   const vec3 V = vec3(M[1][2]-M[2][1], M[2][0]-M[0][2], M[0][1]-M[1][0]);
   vec4 N0 = vec4(M[0][0]+M[0][0], M[1][0]+M[0][1], M[2][0]+M[0][2], V[0]);
   vec4 N1 = vec4(M[0][1]+M[1][0], M[1][1]+M[1][1], M[2][1]+M[1][2], V[1]);
   vec4 N2 = vec4(M[0][2]+M[2][0], M[1][2]+M[2][1], M[2][2]+M[2][2], V[2]);
   N0[0] -= eigenvalue;
   N1[1] -= eigenvalue;
   N2[2] -= eigenvalue;
   N1 -= (N1[0]/N0[0])*N0;
   N2 -= (N2[0]/N0[0])*N0;
   N2 -= (N2[1]/N1[1])*N1;
   const float vez =  N2[3]/N2[2];
   const float vey = (N1[3]-N1[2]*vez)/N1[1];
   const float vex = (N0[3]-N0[2]*vez-N0[1]*vey)/N0[0];
   
   return vec3(vex, vey, vez);
}

void main() {
   
   /// the vertex position
   vec2 coofrag = gl_TexCoord[0].st;
   vec3 p       = texture2D(positions, coofrag).xyz;
   
   /// compute weighted centroids
   vec3 pstar, qstar;
   weightedCentroids(p, pstar, qstar);
   
   /// compute the correlation matrix
   mat3 M = correlationMatrix(p, pstar, qstar);
   
   /// compute the largest root of polynomial and the eigenvalue
   const float traceM = M[0][0] + M[1][1] + M[2][2];
   const float eigenvalue = largestRoot(M) + traceM;
   
   /// compute the rotation vector
   vec3 u = rotationVector(M, eigenvalue);
   
   /// modify the vertex position (apply rotation defined by u)
   vec3 vhat = p - pstar;
   const vec3 tmp1 = cross(vhat,u);
   const vec3 vd  = vhat - (2.0*(cross(u,tmp1) + tmp1))/(1.0+dot(u,u));
   gl_FragData[0] = vec4(qstar+vd, 1.0);
   
   /// modify the vertex normal (apply rotation defined by u)
   vec3 n = texture2D(normals, coofrag).xyz;
   const vec3 tmp2 = cross(n,u);
   const vec3 nd = n - (2.0*(cross(u,tmp2) + tmp2))/(1.0+dot(u,u));
   gl_FragData[1] = vec4(nd, 1.0);
}
